#include "/afs/ihep.ac.cn/users/w/wangbinlong/usefull/PLOT/cepcplotstyle.C"
using namespace ROOT::Math;

double alpha;
double theta;
double injectionx = 0;
double injectiony = 0;
double injectionz = 0;

double X;
double Y;
double Z;
TString gr_title;
double pi = TMath::Pi();

void getpar (TString str,int&, double & );
void Split(TString x, TString delim, vector <TString> &v);
double equation1(double*x, double*p);
double equation2(double*x, double*p);
double equation3(double*x, double*p);
TVector3 getcross1();
TVector3 getcross2();
TVector3 getsparking(TVector3 v1, TVector3 v2, double a, double b);
void WriteComments(double mean, double sigma);
double getmidv(double x, double stepsize);

void analysis10(const char * algo = 0, int printlevel = 0){

	SetStyle();
	TString filename = "mom_5theta_0.root";
	cout<<"filename = "<<filename<<endl;
	int momentum;
	getpar(filename, momentum, theta);

	cout<<"momentum = "<<momentum<<endl;
	theta = theta/180.*pi;
	cout<<"theta = "<<theta<<endl;
	alpha = -pi;
	cout<<"alpha = "<<alpha<<endl;

	cout<<"injectionx = "<<injectionx<<endl;
	cout<<"injectiony = "<<injectiony<<endl;
	cout<<"injectionz = "<<injectionz<<endl;

	double theta2 =TMath::Pi()-theta;
	double phi2 = alpha - TMath::Pi()/2.;
	TVector3 vmu = TVector3(sin(theta2)*cos(phi2), sin(theta2)*sin(phi2), cos(theta2)); 
	cout<<"vmu = "<<vmu.X()<<"  "<<vmu.Y()<<"  "<<vmu.Z()<<endl;
	TVector3 vcross1 = getcross1();
	cout<<"Vcross1 = "<<vcross1.X()<<"  "<<vcross1.Y()<<"  "<<vcross1.Z()<<endl;
	TVector3 vcross2 = getcross2();
	cout<<"Vcross2 = "<<vcross2.X()<<"  "<<vcross2.Y()<<"  "<<vcross2.Z()<<endl;

	TVector3 vmid = getsparking(vcross1, vcross2, 1, 1);
	cout<<"Vmid = "<<vmid.X()<<"  "<<vmid.Y()<<"  "<<vmid.Z()<<endl;

	TFile * file = new TFile (filename);
	file->ls();
	TTree * tree = (TTree*) file->Get("CerenkovPosition");


	tree->SetBranchAddress("X", &X);
	tree->SetBranchAddress("Y", &Y);
	tree->SetBranchAddress("Z", &Z);

	TH1F * hist = new TH1F ("hist", "hist", 18000, 0, 45);
	TH1F * hist2 = new TH1F ("histangle2", "", 180, 0, 0.18);
	TGraph * gr = new TGraph();
	TGraph * gr2 = new TGraph();
	TFile * file2 = new TFile ("file2", "recreate");
	TTree * tree2 = new TTree ("tree", "tree");
	double X_2, Y_2, Z_2;
	tree2->Branch("X_2", &X_2, "X_2/D");
	tree2->Branch("Y_2", &Y_2, "Y_2/D");
	tree2->Branch("Z_2", &Z_2, "Z_2/D");

	TGraph2D * gr2d1 = new TGraph2D();
	TGraph2D * gr2d2 = new TGraph2D();

	///////////////////////////////////////////////////////////
   TCanvas *c1 = new TCanvas("c1","PolyLine3D & PolyMarker3D Window",800, 800);

   // create a pad
   TPad *p1 = new TPad("p1","p1",0.05,0.05,0.95,0.95, kGreen -9);
   p1->Draw();
   p1->cd();
   auto *view = TView::CreateView(1);
   view->SetRange(-0.2,-0.2,-1.5,0.2,0.2,0.5);
   // create a first PolyMarker3D
   TPolyMarker3D *pm3d1 = new TPolyMarker3D(1000);
   TPolyMarker3D *pm3d2 = new TPolyMarker3D(1000);


	///////////////////////////////////////////////////////////
	for (int i = 0; i< tree->GetEntries(); i++){
	cout<<"--------------------------------------------"<<endl;
	if (i>10000) break;
		tree->GetEntry(i);
		//X = getmidv(X, 5);
		//Y = getmidv(Y, 5);
		X/=1000.;
		Y/=1000.;
		Z/=1000.;
		ROOT::Math::MultiRootFinder r(algo);
		//defining the function
		TF1 * f1 = new TF1("f1",equation1,-100,1,0);
		TF1 * f2 = new TF1("f2",equation2,-100,1,0);
		TF1 * f3 = new TF1("f3",equation3,-100,1,0);
		// wrap the functions

		ROOT::Math::WrappedMultiTF1 g1(*f1,3);
		ROOT::Math::WrappedMultiTF1 g2(*f2,3);
		ROOT::Math::WrappedMultiTF1 g3(*f3,3);
		r.AddFunction(g1);
		r.AddFunction(g2);
		r.AddFunction(g3);
		r.SetPrintLevel(printlevel);

		// starting point
		double x0[3]={0,0, -1};
		r.Solve(x0);
		//cout<<x0[0]<<endl;
		//cout<<x0[1]<<endl;
		//cout<<x0[2]<<endl;
		cout<<"x(2) = "<<r.X()[0]<<endl;
		cout<<"y(2) = "<<r.X()[1]<<endl;
		cout<<"z(2) = "<<r.X()[2]<<endl;
		gr2d1->SetPoint(i, X, Y, Z);
		pm3d1->SetPoint(i, X, Y, Z);
		gr->SetPoint(i, X, Y);

		double x_2 = r.X()[0];
		double y_2 = r.X()[1];
		double z_2 = r.X()[2];
		X_2 = x_2;
		Y_2 = y_2;
		Z_2 = z_2;
		gr2d2->SetPoint(i, X_2, Y_2, Z_2);
		pm3d2->SetPoint(i, X_2, Y_2, Z_2);
		gr2->SetPoint(i, X_2, Y_2);
		tree2->Fill();

		double xk = x_2 - injectionx;
		double yk = y_2 - injectiony;
		double zk = z_2 - injectionz;
		cout<<"alpha = "<<alpha<<endl;
		cout<<"theta = "<<theta<<endl;
		cout<<"injectionx = "<<injectionx<<endl;
		cout<<"injectiony = "<<injectiony<<endl;
		//double x2p = cos(theta) * xk + (-sin(theta)*cos(alpha)*yk) + (-sin(theta)*sin(alpha)*zk);
		//double y2p = sin(theta)*xk + (cos(theta)*cos(alpha)*yk) + (cos(theta)*sin(alpha)*zk);
		//double z2p = (-sin(alpha)*yk) + (cos(alpha)*zk);

		double x2p = cos(alpha)*xk + sin(alpha)*yk;
		double y2p = -cos(theta)*sin(alpha)*xk + cos(theta)*cos(alpha)*yk - sin(theta)*zk;
		double z2p = -sin(theta)*sin(alpha)*xk + sin(theta)*cos(alpha)*yk + cos(theta)*zk;
		double theta_c = atan(sqrt((x2p*x2p + y2p*y2p)/(z2p*z2p)));
		cout<<"x2p = "<<x2p<<endl;
		cout<<"y2p = "<<y2p<<endl;
		cout<<"z2p = "<<z2p<<endl;
		cout<<theta_c/3.1415926*180<<" degree"<<endl;
		hist->Fill(theta_c/3.1415926*180.);
		//gr->SetPoint(i, x_2, z_2);
		delete f1;
		delete f2;
		delete f3;
		// test
		double parameters[3];
		double v2[3] = {x_2, y_2, z_2};

		cout<<"equation1   "<<equation1(v2, parameters)<<endl;
		cout<<"equation2   "<<equation2(v2, parameters)<<endl;
		cout<<"equation3   "<<equation3(v2, parameters)<<endl;

		TVector3 vgamma = TVector3(x_2, y_2, z_2) - TVector3(injectionx, injectiony, injectionz);
		cout<<"vgamma = "<<vgamma.X()<<"  "<<vgamma.Y()<<"  "<<vgamma.Z()<<endl;
		double angle = vmu.Angle(vgamma);
		cout<<"angle = "<<angle/TMath::Pi()*180.<<endl;

		// let the gamma come from the vmid;
		TVector3 vgamma2 = TVector3(x_2, y_2, z_2) - vmid;
		cout<<"vgamma2 = "<<vgamma2.X()<<"  "<<vgamma2.Y()<<"  "<<vgamma2.Z()<<endl;
		double angle2 = vmu.Angle(vgamma2);
		//angle2 = angle2/TMath::Pi()*180;
		cout<<"angle2 = "<<angle2/TMath::Pi()*180.<<endl;
		hist2->Fill(angle2);

	}
	TCanvas * can1 = new TCanvas ("can1", "can1", 800, 800);
	hist->Draw();
	hist->GetXaxis()->SetTitle("theta_{c} [degree]");
	hist->GetYaxis()->SetTitle("Entries");
	TCanvas * can2 = new TCanvas ("can2", "can2", 800, 800);
	gr2->Draw("AP");
	gr->SetMarkerStyle(20);
	gr->SetMarkerSize(0.5);
	gr->SetMarkerColor(kRed);
	gr->Draw("Psame");
	gr2->SetMarkerStyle(20);
	gr2->SetMarkerSize(0.5);
	gr2->SetMarkerColor(kBlue);
	
	TH3F * hist3 = new TH3F ("hist3", "hist3", 1, -2, 2, 1, -2, 2, 1, -2, 2);
	TCanvas * can3 = new TCanvas ("can3", "can3", 800, 800);
	//hist3->Draw();
	gr2d2->Draw();
	gr2d2->SetMaximum(-0.9);
	gr2d1->Draw("Psame");
	gr2d1->SetMarkerColor(kRed);
	gr2d2->SetMarkerColor(kBlue);
	gr2d1->SetMarkerStyle(20);
	gr2d1->SetMarkerSize(0.5);
	gr2d2->SetMarkerStyle(20);
	gr2d2->SetMarkerSize(0.5);

	///////////////////////////////////////////////////////////
	c1->cd();
	p1->cd();
	//pm3d1->SetMarkerSize(1);
	pm3d1->SetMarkerColor(kRed);
	//pm3d1->SetMarkerStyle(2);
	//pm3d2->SetMarkerSize(2);
	pm3d2->SetMarkerColor(kBlue);
	//pm3d2->SetMarkerStyle(8);
	pm3d1->Draw();
	pm3d2->Draw();


	///////////////////////////////////////////////////////////
	TCanvas * can_angle2 = new TCanvas ("can_angle2", "can_angle2", 800, 800);
	hist2->Fit("gaus", "", "", hist2->GetMean()-hist2->GetRMS()*1.5, hist2->GetMean()+hist2->GetRMS()*1.5);
	//hist2->Fit("gaus", "", "", 0.125, 0.144);
	//TFitResultPtr r = hist2->Fit("gaus");
	hist2->Draw();
	hist2->GetXaxis()->SetTitle("#theta_{c} rec [rad]");
	hist2->GetYaxis()->SetTitle("Entries/0.1mrd");
	//double Gaus_mean = r->Parameter(1);
	//double Gaus_mean_err = r->ParError(1);
	//double Gaus_sigma = r->Parameter(2);
	//double Gaus_sigma_err = r->ParError(2);
	//WriteComments(Gaus_mean, Gaus_sigma);

	file2->cd();
	tree2->Write();
	file2->Close();
	cout<<"++++++++++++++++"<<endl;
	cout<<"RMS = " <<hist2->GetRMS()<<endl; 
}

void getpar (TString str, int & momentum, double & tmptheta){
	str.ReplaceAll(".root", "");
	str.ReplaceAll("mom", "");
	str.ReplaceAll("theta", "");
	str.ReplaceAll("alpha", "");
	str.ReplaceAll("x", "");
	str.ReplaceAll("y", "");
	//cout<<str<<endl;
	vector <TString> v;
	Split(str, "_", v);
	//for (int i = 0; i< v.size(); i++){
	//	cout<<i<<"  "<<v[i]<<endl;
	//}
	momentum = v[1].Atoi();
	tmptheta = v[2].Atof();

}

void Split(TString x, TString delim, vector <TString> &v){
	v.clear();
	bool ifprint = 0;

	int stringLength=x.Length();
	int delimLength=delim.Length();

	if(ifprint)cout<<"String: "<<x<<"  "<<stringLength<<endl;
	if(ifprint)cout<<"Delim: "<<delim<<"  "<<delimLength<<endl;

	if(ifprint)cout<<x<<"  "<<stringLength<<endl;

	int stop=1;
	TString temp="---";
	while(stop!=-1){
		stop = x.First(delim);

		if(stop!=-1){
			temp = x(0, stop);
			if(ifprint)cout<<"Substring: "<<temp<<endl;
			TSubString newString = x( stop+delimLength, stringLength );
			x=newString;
			stringLength=x.Length();
		}
		else{
			stringLength=x.Length();
			temp = x(0, stringLength);
			if(ifprint)cout<<"Substring: "<<temp<<endl;
		}

		v.push_back(temp);
	}
}

double equation1(double*x, double*p){
	double x2 = x[0];
	double y2 = x[1];
	double z2 = x[2];
	//cout<<x2<<"  "<<y2<<"  "<<z2<<endl;

	double x1 = injectionx;
	double y1 = injectiony;
	double z1 = injectionz;
	double l = -1;

	double tmp1 = -x1 + 0.5*x2*(x1*x2 + y1*y2 + z1*z2);
	double tmp2 = z1 + 0.5 *(-(x2*x2 + y2*y2)*z1 + (x1*x2 + y1*y2)*z2);
	double kk = -X + tmp1 + (x2-tmp1)*(l-tmp2)/(z2-tmp2);
	return kk;
}
double equation2(double*x, double*p){
	double x2 = x[0];
	double y2 = x[1];
	double z2 = x[2];

	double x1 = injectionx;
	double y1 = injectiony;
	double z1 = injectionz;
	double l = -1;

	double tmp1 = -y1 + 0.5*y2*(x1*x2 + y1*y2 + z1*z2);
	double tmp2 = z1 + 0.5 *(-(x2*x2 + y2*y2)*z1 + (x1*x2 + y1*y2)*z2);
	double kk = -Y + tmp1 + (y2-tmp1)*(l-tmp2)/(z2-tmp2);
	return kk;
}

double equation3(double*x, double*p){
	double x2 = x[0];
	double y2 = x[1];
	double z2 = x[2];
	return 4-x2*x2 -y2*y2-z2*z2;
}

TVector3 getcross2(){

	double theta2 =TMath::Pi()-theta;
	double phi2 = alpha - TMath::Pi()/2.;
	TVector3 vmu = TVector3(sin(theta2)*cos(phi2), sin(theta2)*sin(phi2), cos(theta2)); 
	double stepx = vmu.X()/10000;
	double stepy = vmu.Y()/10000;
	double stepz = vmu.Z()/10000;
	double x;
	double y;
	double z;

	for (int i = 0; i < 100000; i++){
		x = injectionx + 3.*i*stepx;
		y = injectiony + 3.*i*stepy;
		z = injectionz + 3.*i*stepz;
		double length = sqrt(x*x + y*y + z*z); 
		if (length>2) break;
	}
	return TVector3 (x, y, z);

}
TVector3 getcross1(){

	double theta2 =TMath::Pi()-theta;
	double phi2 = alpha - TMath::Pi()/2.;
	TVector3 vmu = TVector3(sin(theta2)*cos(phi2), sin(theta2)*sin(phi2), cos(theta2)); 
	double stepx = vmu.X()/10000;
	double stepy = vmu.Y()/10000;
	double stepz = vmu.Z()/10000;
	double x;
	double y;
	double z;

	for (int i = 0; i < 100000; i++){
		x = injectionx + 3.*i*stepx;
		y = injectiony + 3.*i*stepy;
		z = injectionz + 3.*i*stepz;
		if (z<-1) break;
	}
	return TVector3 (x, y, z);

}

void WriteComments(double mean, double sigma){
	TPaveText *pt = new TPaveText(.05,.1,.95,.8, "NDC");

	pt->AddText(gr_title);
	pt->AddLine(.0,.5,1.,.5);
	pt->AddText(Form("Gaus::Mean %5.3f", mean));
	pt->AddText(Form("Gaus::Sigma %5.3f", sigma));

	pt->Draw();
}

TVector3 getsparking(TVector3 v1, TVector3 v2, double a, double b){
	return v1*(b/(a+b)) + v2 * (a/(a+b));
}
double getmidv(double x, double stepsize){
	double xp;
	if (x>0) xp = (int) (x/stepsize) *2 + (double) stepsize/2.;
	else  xp = (int) (x/stepsize) *2 - (double) stepsize/2.;
	return xp;
}
